#include "svpwm.h"
#include "math/fast_math.h"
#include "utils/utils.h"
#include "config.h"

#define SECTOR_UKNOW 0xFF

u8 svpwm_get_duty(float alpha, float beta, float *duty) {
	uint8_t sector = 0xFF;
	float   tA, tB, tC;
	float   X, Y, Z;

	if (beta >= 0) {
		if (alpha >= 0) {
			//quadrant I
			if ((ONE_BY_SQRT3 * beta) > alpha) {
				sector = SECTOR_2;
			} else {
				sector = SECTOR_1;
			}
		} else {
			//quadrant II
			if ((-ONE_BY_SQRT3 * beta) > alpha) {
				sector = SECTOR_3;
			} else {
				sector = SECTOR_2;
			}
		}
	} else {
		if (alpha >= 0) {
			//quadrant IV5
			if ((-ONE_BY_SQRT3 * beta) > alpha) {
				sector = SECTOR_5;
			} else {
				sector = SECTOR_6;
			}
		} else {
			//quadrant III
			if ((ONE_BY_SQRT3 * beta) > alpha) {
				sector = SECTOR_4;
			} else {
				sector = SECTOR_5;
			}
		}
	}

	X = (SQRT3 * beta);
	Y = 1.5f * alpha + (SQRT3_BY_2 * beta);
	Z = -1.5f * alpha + (SQRT3_BY_2 * beta);
	switch(sector) {
		case SECTOR_1: // 3
		{	
			// Vector on-times
            float T4 = -Z;
			float T6 = X;
		#if CONFIG_PWM_HIGH_FIRST
			tC = 0.5f * (1.0f - T4 - T6);
        	tB = tC + T6;
        	tA = tB + T4;
		#else
			tA = 0.5f * (1.0f - T4 - T6);
			tB = tA + T4;
			tC = tB + T6;
		#endif
			break;
		}
		case SECTOR_2: // 1
		{
            // Vector on-times
			float T6 = Y;
			float T2 = Z;
		#if CONFIG_PWM_HIGH_FIRST
			tC = 0.5f * (1.0f - T6 - T2);
        	tA = tC + T6;
        	tB = tA + T2;
		#else
			tB = 0.5f * (1.0f - T6 - T2);
			tA = tB + T2;
			tC = tA + T6;
		#endif
			break;
		}
		case SECTOR_3: // 5
		{
            // Vector on-times
			float T2 = X;
			float T3 = -Y;
		#if CONFIG_PWM_HIGH_FIRST
			tA = 0.5f * (1.0f - T2 - T3);
			tC = tA + T3;
			tB = tC + T2;
		#else
			tB = 0.5f * (1.0f - T2 - T3);
			tC = tB + T2;
			tA = tC + T3;
		#endif
			break;
		}
		case SECTOR_4: // 4
		{
            // Vector on-times
			float T1 = -X;
			float T3 = Z;
		#if CONFIG_PWM_HIGH_FIRST
			tA = 0.5f * (1.0f - T1 - T3);
			tB = tA + T3;
			tC = tB + T1;
		#else
			tC = 0.5f * (1.0f - T1 - T3);
			tB = tC + T1;
			tA = tB + T3;
		#endif
			break;
		}
		case SECTOR_5: // 6
		{
            // Vector on-times
			float T1 = -Y;
			float T5 = -Z;
		#if CONFIG_PWM_HIGH_FIRST
			tB = 0.5f * (1.0f - T1 - T5);
			tA = tB + T5;
			tC = tA + T1;
		#else
			tC = 0.5f * (1.0f - T1 - T5);
			tA = tC + T1;
			tB = tA + T5;
		#endif
			break;
		}					
		case SECTOR_6: // 2
		{
            // Vector on-times
			float T4 = Y;
			float T5 = -X;
		#if CONFIG_PWM_HIGH_FIRST
			tB = 0.5f * (1.0f - T4 - T5);
			tC = tB + T5;
			tA = tC + T4;
		#else
			tA = 0.5f * (1.0f - T4 - T5);
			tC = tA + T4;
			tB = tC + T5;
		#endif
			break;
		}
		default:
			break;
	}	
	duty[0] = clamp_f(tA, 0, 1.0f);
	duty[1] = clamp_f(tB, 0, 1.0f);
	duty[2] = clamp_f(tC, 0, 1.0f);
	return sector;
}
